//QUIRKSIM
//Copyright (c) 2008 Gregory Andrew Robinson (garobinson@ucdavis.edu)
//
//Permission is hereby granted, free of charge, to any person
//obtaining a copy of this software and associated documentation
//files (the "Software"), to deal in the Software without
//restriction, including without limitation the rights to use,
//copy, modify, merge, publish, distribute, sublicense, and/or sell
//copies of the Software, and to permit persons to whom the
//Software is furnished to do so, subject to the following
//conditions:
//
//The above copyright notice and this permission notice shall be
//included in all copies or substantial portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
//
//EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
//OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
//NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
//WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
//FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
//OTHER DEALINGS IN THE SOFTWARE.

#ifndef _PARTICLE_TEST_CC
#define _PARTICLE_TEST_CC

#include <iostream>
#include <stdexcept>
#include <gtest/gtest.h>
#include "particle.h"

namespace test {

using quirksim::particle::MassiveParticle;
using CLHEP::Hep3Vector;

testing::AssertionResult AssertHep3VectorEq(const char* v1_expr,
                                            const char* v2_expr,
                                            const char* eps_expr,
                                            Hep3Vector expected,
                                            Hep3Vector actual,
                                            double epsilon) {
  if (expected.isNear(actual, epsilon)) {
    return testing::AssertionSuccess();
  }
  testing::Message msg;
  msg << v1_expr << " and " << v2_expr << " (<" << expected.x() << ", "
      << expected.y() << ", " << expected.z() << ">" << " and <" << actual.x()
      << ", " << actual.y() << ", " << actual.z()
      << ">) are not within epsilon=" << epsilon
      << " in one or more dimensions.";
  return testing::AssertionFailure(msg);
}

const Hep3Vector INITIAL_POSITION = Hep3Vector(0.0, 0.0, 0.0);
const Hep3Vector INITIAL_MOMENTUM = Hep3Vector(0.0, 0.0, 0.0);

// Initial conditions used for testing.
const double TEST_MASS = 350e6;
const Hep3Vector TEST_POSITION = Hep3Vector(4.0, 5.0, 7e6);
const Hep3Vector TEST_MOMENTUM = Hep3Vector(3e8, 4e9, 5e10);

class MassiveParticleTest : public testing::Test {
protected:

  MassiveParticleTest() : p(TEST_MASS) {
    p.set_position(TEST_POSITION);
    p.set_momentum(TEST_MOMENTUM);
  }

  quirksim::particle::MassiveParticle p;
};

TEST(MassiveParticleConstructorTest, CorrectMass) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  EXPECT_EQ(TEST_MASS, p1.mass());
  quirksim::particle::MassiveParticle p2(TEST_MASS, INITIAL_POSITION,
                                INITIAL_MOMENTUM);
  EXPECT_EQ(TEST_MASS, p2.mass());
}

TEST(MassiveParticleConstructorTest, CorrectPosition) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, INITIAL_POSITION, p1.position(), 0);
  // Make sure it creates a new object.
  EXPECT_NE(&INITIAL_POSITION, &p1.position());
  quirksim::particle::MassiveParticle p2(TEST_MASS, INITIAL_POSITION,
                                INITIAL_MOMENTUM);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, INITIAL_POSITION, p2.position(), 0);
  EXPECT_NE(&INITIAL_POSITION, &p2.position());
}

TEST(MassiveParticleConstructorTest, CorrectMomentum) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, INITIAL_MOMENTUM, p1.momentum(), 0);
  EXPECT_NE(&INITIAL_MOMENTUM, &p1.momentum());
  quirksim::particle::MassiveParticle p2(TEST_MASS, INITIAL_POSITION,
                                INITIAL_MOMENTUM);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, INITIAL_MOMENTUM, p2.momentum(), 0);
  EXPECT_NE(&INITIAL_MOMENTUM, &p2.momentum());
}

TEST_F(MassiveParticleTest, set_position) {
  const Hep3Vector pos = Hep3Vector(5.3, 42, 5e6);
  p.set_position(pos);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, pos, p.position(), 0);
  EXPECT_NE(&pos, &p.position());
}

TEST_F(MassiveParticleTest, set_momentum) {
  const Hep3Vector mom = Hep3Vector(5.3, 42, 5e6);
  p.set_momentum(mom);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, mom, p.momentum(), 0);
  EXPECT_NE(&mom, &p.momentum());
}

class ConstantTestForce : public quirksim::particle::ForceInterface {
public:

  ConstantTestForce(double a, double b, double c) : f(a, b, c) {
  }

  Hep3Vector GetForce(const Hep3Vector& x, const Hep3Vector& v) const {
    return GetForce();
  }

  Hep3Vector GetForce() const {
    return f;
  }
private:
  Hep3Vector f;
};

TEST_F(MassiveParticleTest, Forces) {
  ConstantTestForce* f1 = new ConstantTestForce(0.0, 0.0, 0.0);
  ConstantTestForce* f2 = new ConstantTestForce(1.0, 2.4, 5.0);
  ConstantTestForce* f3 = new ConstantTestForce(3.6, 5.0, 1.1);

  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0.0, 0.0, 0.0), p.GetNetForce(), 0);
  p.RegisterForce(f1);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0.0, 0.0, 0.0), p.GetNetForce(), 0);
  p.RegisterForce(f2);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(1.0, 2.4, 5.0), p.GetNetForce(), 0);
  p.RegisterForce(f3);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(4.6, 7.4, 6.1), p.GetNetForce(), 0);

}

TEST_F(MassiveParticleTest, GetEnergy) {
  EXPECT_DOUBLE_EQ(5.016186300368040e+10, p.GetEnergy());
  EXPECT_DOUBLE_EQ(p.mass(), p.GetEnergy(Hep3Vector(0, 0, 0)));
}

TEST_F(MassiveParticleTest, GetVelocity) {
  double eps = 1e-15;
  const Hep3Vector v_expected =
      Hep3Vector(0.005980639115776, 0.079741854877011, 0.996773185962640);
  Hep3Vector v_actual = p.GetVelocity();
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, v_expected, p.GetVelocity(), eps);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(0, 0, 0), p.GetVelocity(Hep3Vector(0, 0, 0)), eps);
}

TEST(MassiveParticleEvolveTest, ConstantNewtonian) {
  const double mass = 1.0;
  quirksim::particle::MassiveParticle p(mass);

  //Time step for one iteration.
  double dt = 1.0;

  double fx = 0;
  double fy = 5.0;
  double fz = 9.8;
  double ax = fx / mass;
  double ay = fy / mass;
  double az = fz / mass;

  ConstantTestForce* f = new ConstantTestForce(fx, fy, fz);
  p.RegisterForce(f);
  p.Evolve(dt);

  const Hep3Vector p_exact = Hep3Vector(fx * dt, fy * dt, fz * dt);
  const Hep3Vector v_exact = Hep3Vector(ax * dt, ay * dt, az * dt);
  const Hep3Vector x_exact = Hep3Vector(0.5 * ax * dt * dt,
                                        0.5 * ay * dt * dt,
                                        0.5 * az * dt * dt);

  const double epsilon = 1e-14;
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, p_exact, p.momentum(), epsilon);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, v_exact, p.GetVelocity(), epsilon);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, x_exact, p.position(), epsilon);
}

TEST(StraightQuirkStringTest, GetDirection) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  quirksim::particle::MassiveParticle p2(TEST_MASS, TEST_POSITION, INITIAL_MOMENTUM);
  quirksim::particle::MassiveParticle p3(TEST_MASS);
  quirksim::particle::StraightQuirkString qs =
      quirksim::particle::StraightQuirkString(p1, p2);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, TEST_POSITION.unit(), qs.GetDirection(p1), 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, -TEST_POSITION.unit(), qs.GetDirection(p2), 0);
  EXPECT_THROW(qs.GetDirection(p3), std::invalid_argument);
}

TEST(StraightQuirkStringTest, GetLength) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  quirksim::particle::MassiveParticle p2(TEST_MASS);
  p1.set_position(Hep3Vector(10, 10, 10));
  quirksim::particle::StraightQuirkString qs =
      quirksim::particle::StraightQuirkString(p1, p2);
  EXPECT_EQ(17.320508075688775, qs.GetLength());
}

TEST(QuirkPairTest, GetDirection) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  quirksim::particle::MassiveParticle p2(TEST_MASS, TEST_POSITION, INITIAL_MOMENTUM);
  quirksim::particle::MassiveParticle p3(TEST_MASS);
  double sigma = 42;
  quirksim::particle::QuirkPair pair(p1, p2, sigma);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, TEST_POSITION.unit(), pair.GetDirection(p1), 0);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, -TEST_POSITION.unit(), pair.GetDirection(p2), 0);
  EXPECT_THROW(pair.GetDirection(p3), std::invalid_argument);
}

TEST(QuirkPairTest, sigma) {
  quirksim::particle::MassiveParticle p1(TEST_MASS);
  quirksim::particle::MassiveParticle p2(TEST_MASS, TEST_POSITION, INITIAL_MOMENTUM);
  double sigma = 42;
  quirksim::particle::QuirkPair pair(p1, p2, sigma);
  EXPECT_EQ(sigma, pair.sigma());
}

TEST(QuirkForceTest, GetForce) {
  quirksim::particle::MassiveParticle p1(TEST_MASS, Hep3Vector(-1, 0, 0), TEST_MOMENTUM);
  quirksim::particle::MassiveParticle p2(TEST_MASS, Hep3Vector(1, 0, 0), TEST_MOMENTUM);
  double sigma = 42;
  quirksim::particle::QuirkPair pair(p1, p2, sigma);
  quirksim::particle::QuirkForce f1(pair, p1);
  quirksim::particle::QuirkForce f2(pair, p1);

  Hep3Vector expected = Hep3Vector(36.373066958946424, -12.124355652982142573, 0);
  Hep3Vector actual = f1.GetForce(p1.position(), Hep3Vector(-.5, .5, 0));
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, expected,
                      f2.GetForce(p1.position(), Hep3Vector(-.5, .5, 0)), 1e-14);

  expected = Hep3Vector(-36.373066958946424, -12.124355652982142573, 0);
  actual = f2.GetForce(p2.position(), Hep3Vector(.5, .5, 0));
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, expected,
                      f2.GetForce(p2.position(), Hep3Vector(.5, .5, 0)), 1e-14);
}

TEST(Hep3VectorTest, project) {
  Hep3Vector v1(1, 2, 3);
  Hep3Vector v2(1, 0, 0);
  Hep3Vector expected(1, 0, 0);
  Hep3Vector actual = v1.project(v2);
  EXPECT_PRED_FORMAT3(AssertHep3VectorEq, Hep3Vector(1, 0, 0), v1.project(v2), 0);
}

} //namespace test
#endif
